#include <stdint.h>
#include <config.h>
#include <mm/mem.h>
#include "rp2040.h"
#include "resets.h"

#define BIT(n)   (1 << (n))

/**
 *  @brief  SRAM memory information
*/
#define  SRAM_MAIN_START_ADDR        (0x20000000)
#define  SRAM_MAIN_SIZE              (CONFIG_SRAM_SIZE)


// System clock
// FREQUENCY = ((FREF / REFDIV) × FBDIV / (POSTDIV1 × POSTDIV2))
//          REF    FBDIV   VCO      PDIV1   PDIV2   RESULT
// SYS PLL: 12MHz * 133 = 1596MHz /   6   /   2  =  133MHz
// USB PLL: 12MHz * 100 = 1200MHz /   5   /   5  =  48MHz
enum { F_REF = 12000000, F_REFDIV = 1 };
enum { F_SYS_FBDIV = 200, F_SYS_POSTDIV1 = 6, F_SYS_POSTDIV2 = 2 };
enum { F_USB_FBDIV = 100, F_USB_POSTDIV1 = 5, F_USB_POSTDIV2 = 5 };
#define F_SYS \
  ((F_REF / F_REFDIV) * F_SYS_FBDIV / (F_SYS_POSTDIV1 * F_SYS_POSTDIV2))
#define F_USB \
  ((F_REF / F_REFDIV) * F_USB_FBDIV / (F_USB_POSTDIV1 * F_USB_POSTDIV2))


struct resets {
  volatile uint32_t RESET, WDSEL, DONE;
};

#define RESETS ((struct resets *) 0x4000c000)

struct xosc {
  volatile uint32_t CTRL, STATUS, DORMANT, STARTUP, RESERVED[3], COUNT;
};
#define XOSC ((struct xosc *) 0x40024000)

struct pll {
  volatile uint32_t CS, PWR, FBDIV_INT, PRIM;
};
#define PLL_SYS ((struct pll *) 0x40028000)
#define PLL_USB ((struct pll *) 0x4002c000)

struct clocks {
  struct {
    volatile uint32_t CTRL, DIV, SELECTED;
  } GPOUT0, GPOUT1, GPOUT2, GPOUT3, REF, SYS, PERI, USB, ADC, RTC;
  volatile uint32_t SYS_RESUS_CTRL, SYS_RESUS_STATUS, FC0_REF_KHZ, FC0_MIN_KHZ,
      FC0_MAX_KHZ, FC0_DELAY, FC0_INTERVAL, FC0_SRC, FC0_STATUS, FC0_REST,
      WAKE_EN0, WAKE_EN1, SLEEP_EN0, SLEEP_EN1, ENABLED0, ENABLED1, INTR, INTE,
      INTF, INTS;
};
#define CLOCKS ((struct clocks *) 0x40008000)

#define CTRL_ENABLE 			(0xFAB << 12)
#define CTRL_DISABLE			(0xD1E << 12)
#define CTRL_FREQ_1_15_MHZ		0xAA0

#define STATUS_STABLE			(1 << 31)
#define STATUS_BADWRITE 		(1 << 24)
#define STATUS_ENABLED			(1 << 12)
#define STATUS_FREQ_1_15_MHZ	(0 << 0)

#define DORMANT_WAKE			0x77616B65
#define DORMANT_DORMANT			0x636F6D61

static inline void enable_subsystem(uint32_t bits) {
	RESETS->RESET &= ~bits;                       // Unreset subsystem
	while ((RESETS->DONE & bits) == 0) (void) 0;  // Wait until done
}


static void sys_clock_init(void) 
{
  IO_WR(CLOCKS_BASE + 0x48, 0x880); // Enable clk_peri

  XOSC->CTRL = 2720;         // XOSC frequency range 1-15 MHz
  XOSC->STARTUP = 47;        // About 1 ms, see 2.16.3
  XOSC->CTRL |= 4011 << 12;  // Enable XOSC
  while ((XOSC->STATUS & BIT(31)) == 0) (void) 0;  // Wait until enabled

  enable_subsystem(BIT(12));  // Reset SYS PLL
  PLL_SYS->FBDIV_INT = F_SYS_FBDIV;
  PLL_SYS->PRIM = (F_SYS_POSTDIV1 << 16) | (F_SYS_POSTDIV2 << 12);
  PLL_SYS->PWR &= ~(BIT(0) | BIT(3) | BIT(5));    // Power up
  while ((PLL_SYS->CS & BIT(31)) == 0) (void) 0;  // Wait

  enable_subsystem(BIT(13));  // Reset USB PLL
  PLL_USB->FBDIV_INT = F_USB_FBDIV;
  PLL_USB->PRIM = (F_USB_POSTDIV1 << 16) | (F_USB_POSTDIV2 << 12);
  PLL_USB->PWR &= ~(BIT(0) | BIT(3) | BIT(5));    // Power up
  while ((PLL_USB->CS & BIT(31)) == 0) (void) 0;  // Wait

  CLOCKS->REF.CTRL = (2 << 0);             // REF source is XOSC
  CLOCKS->SYS.CTRL = (0 << 5) | (1 << 0);  // SYS source is CLKSYS_AUX
  CLOCKS->PERI.CTRL = BIT(11) | (0 << 5);  // PERI clock enable, source SYS
  CLOCKS->USB.CTRL = BIT(11) | (0 << 5);   // USB clock enable, source USB PLL
  CLOCKS->ADC.CTRL = BIT(11) | (0 << 5);   // ADC clock enable, source USB PLL
  CLOCKS->RTC.DIV = (48 << 8);             // RTC divider: 12 / 48 = 0.25Mhz
  CLOCKS->RTC.CTRL = BIT(11) | (3 << 5);   // RTC clock enable, source XOSC
  enable_subsystem(BIT(5) | BIT(8));  // IO_BANK0 and PADS_BANK0

  reset_release_wait(RESET_IO_BANK0);
	reset_release_wait(RESET_PADS_BANK0);
}


extern size_t eb_system_heap;

static void heap_init(void)
{
  void* start = &eb_system_heap;
	kmem_init(start, CONFIG_SRAM_SIZE - CONFIG_STACK_SIZE - ((uint32_t)start - SRAM_MAIN_START_ADDR));
}


void eb_machine_init(void)
{
	sys_clock_init();
	heap_init();
}
